/* 
 * Name       : prosper_user_code.cpp
 * Author     : Xiaoafei
 * Copyright  : 2017 The Prosper Tech
 *
 * Brief      : 成都普诺思博科技有限公司通用移动平台上层通信接口 v1.0
 */
#include "turtlebot_user_code.h"
 
namespace turtlebot_user {

    TurtlebotUser::TurtlebotUser(std::string name) :
            driver_name_(name),
            RobotDriverBase(name) {

        //TrailerModeCtl(1,0.7,0.6);
        ros::NodeHandle nh_p = ros::NodeHandle("~");
        nh_private_ = ros::NodeHandle(nh_p, name);

//        serial_port_ = nh_private_.param("serial_port",std::string("/dev/ttyUSB0"));
//        baud_rate_ = nh_private_.param("baud_rate",115200);
//        sp_ = new boost::asio::serial_port(io_s_,serial_port_);
//        sp_->set_option(boost::asio::serial_port::baud_rate(baud_rate_));
//        sp_->set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none));
//        sp_->set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
//        sp_->set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
//        io_s_.run();
//        sp_->set_option(boost::asio::serial_port::character_size(8));
        arm_working_ = true;
        arm_working_th_ = boost::thread(boost::bind(&TurtlebotUser::arm_working_exec,this));


        laser_sub_ = nh_.subscribe("/scan",20,&TurtlebotUser::laserposeCallBack, this);
        /*发布底盘速度控制消息*/
        vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);

        /*订阅里程计*/
        odom_sub_ = nh_.subscribe("odom", 1, &TurtlebotUser::odomCallBack, this);
        /*订阅世界坐标*/
        world_pose_sub_ = nh_.subscribe("CarPoseUpdate", 1, &TurtlebotUser::worldPoseUpdateCallBack, this);

        /*发布底盘位置调整请求*/
        pose_adjust_request_pub_ = nh_.advertise<prosper_underpan::PoseRequestInfo>("PoseRequest", 1);
        /*订阅底盘位置调整完成响应*/
        pose_adjust_ack_sub_ = nh_.subscribe("PoseAck", 1, &TurtlebotUser::poseAckInfoCallBack, this);

        /*订阅群控系统自定义事件请求*/
        group_control_sub_ = nh_.subscribe("GroupControlRequest", 1, &TurtlebotUser::groupControlRequestCallBack,
                                                   this);
        /*发布群控系统自定义事件完成响应*/
        group_control_pub_ = nh_.advertise<prosper_underpan::GroupControlInfo>("GroupControlAck", 1);

        /*订阅紧急情况的消息*/
        emergent_info_sub_ = nh_.subscribe("EmergentInfo", 1, &TurtlebotUser::emergentInfoCallBack, this);
    }

    TurtlebotUser::~TurtlebotUser() {
      sp_->close();
      arm_working_ = false;
      arm_working_th_.join();
      delete sp_;
    }


    void TurtlebotUser::laserposeCallBack(const sensor_msgs::LaserScanConstPtr& laser){
      std::vector<long> laser_data;
      for(int i=0;i<laser->ranges.size();i++)
      {
        if(laser->ranges[i] > 0.4 && laser->ranges[i] < 6.0){
          laser_data.push_back(long(laser->ranges[i] * 1000));
        }else{
          laser_data.push_back(0.0);
        }
      }
      stamp_ = laser->header.stamp;
      frame_id_ = laser->header.frame_id;


      tf::StampedTransform transform_laser;
      tf::Transform laser_base1;

      tf::Vector3 point;
      tf::Vector3 point_base;
      std::vector<tf::Vector3> points_base;

         if(listener_.waitForTransform("map",frame_id_,ros::Time(0),ros::Duration(0.2))){
           listener_.lookupTransform("map",frame_id_,ros::Time(0),transform_laser);

           Eigen::Isometry3d t1 = Eigen::Isometry3d::Identity();
           double* dt1 =  t1.data();

           dt1[0]=transform_laser.getBasis()[0][0];
           dt1[1]=transform_laser.getBasis()[1][0];
           dt1[2]=transform_laser.getBasis()[2][0];
           dt1[3]=0;
           dt1[4]=transform_laser.getBasis()[0][1];
           dt1[5]=transform_laser.getBasis()[1][1];
           dt1[6]=transform_laser.getBasis()[2][1];
           dt1[7]=0;
           dt1[8]=transform_laser.getBasis()[0][2];
           dt1[9]=transform_laser.getBasis()[1][2];
           dt1[10]=transform_laser.getBasis()[2][2];
           dt1[11]=0;
           dt1[12]=transform_laser.getOrigin().x();
           dt1[13]=transform_laser.getOrigin().y();
           dt1[14]=0;
           dt1[15]=1;

           for(int i=0;i<laser_data.size();i++){
            // float angle=3.14159*(((float)180/721)*(float)i)/180.0;
             float angle=3.14159*(((float)180/721)*(float)i)/180.0+((1.5707963-1.207963)/1.5707963)*(3.14159/2);
             //float angle = 3.14159*(-45+(270/721)*(double)i)/180;

             Eigen::Vector4d trans1;
             double *tran=trans1.data();
             tran[0]=laser_data[i]*sin(angle)/1000;
             tran[1]=-laser_data[i]*cos(angle)/1000;
             tran[2]=0;
             tran[3]=1;
             Eigen::Vector4d pose=t1*trans1;

             tf::Vector3 aa;
             aa.setX(pose[0]);
             aa.setY(pose[1]);
             aa.setZ(0);

//             tf::Vector3 cc;
//             cc = global_pose_.inverse()*aa;

             points_base.push_back(aa);
           }
           if(!robot_laser_pose_callback_.empty()){
             robot_laser_pose_callback_(points_base);
           }
         }

    }

/***************************************************************************************************** 
 * Function:       // PubVel 
 * Description:    // 底盘速度控制接口                 
 * Publisher:      // vel_pub
 * Topic:          // "cmd_vel"
 * Input:          // float vx: 底盘x轴速度(前进后退，前进为证，范围：-1.5m/s~1.5m/s)
 *                 // float vy: 底盘y轴速度(左右平移，向左为正，范围：-1.5m/s~1.5m/s)
 *                 // float vth:底盘z轴旋转速度(绕中心旋转，逆时针为正,范围：-1.57rad/s~1.57rad/s)
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 速度控制大小应在合理范围内；一条速度指令最多持续150ms,如果要连续控制，两帧速度命令至少间隔100ms
*******************************************************************************************************/
    bool TurtlebotUser::pubVel(float vx, float vy, float vth) {
        geometry_msgs::Twist vel_cmd;
        vel_cmd.linear.x = vx;
        vel_cmd.linear.y = vy;
        vel_cmd.angular.z = vth;
        vel_pub_.publish(vel_cmd);
        return true;
    }

/***************************************************************************************************** 
 * Function:       // odomCallBack 
 * Description:    // 里程计回调函数，用于得到从底盘上电开始，机器人利用内部传感器推算出的在环境的坐标                 
 * Subscriber:     // odom_sub
 * Topic:          // "odom"
 * Input:          // nav_msgs::Odometry::ConstPtr &msg: 订阅得到的里程计消息
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 里程计在地毯环境、底盘打滑等情况下可能误差较大
*******************************************************************************************************/
    void TurtlebotUser::odomCallBack(const nav_msgs::Odometry::ConstPtr &msg) {
//    double odom_pose[3];
//    odom_pose[0] = msg->pose.pose.position.x;
//    odom_pose[1] = msg->pose.pose.position.y;
//    odom_pose[2] = angle_normalize(tf::getYaw(msg->pose.pose.orientation));

        if (!odom_callback_.empty()) {
            odom_callback_(msg);
        }
    }

/***************************************************************************************************** 
 * Function:       // worldPoseUpdateCallback 
 * Description:    // 机器人世界坐标回调函数，地图构建、巡航模式下会回调此函数，得到机器人的世界坐标                 
 * Subscriber:     // world_pose_sub
 * Topic:          // "CarPoseUpdate"
 * Input:          // prosper_underpan::CarPoseInfo::ConstPtr& msg: 位置消息
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 机器人世界坐标一般只在巡航模式下有用，机器人在地图上定位完成后，会不断输出在环境中的真实位置
*******************************************************************************************************/
    void TurtlebotUser::worldPoseUpdateCallBack(const prosper_underpan::CarPoseInfo::ConstPtr &msg) {
        double world_pose[3];
        world_pose[0] = msg->x;
        world_pose[1] = msg->y;
        world_pose[2] = angle_normalize(msg->th * M_PI / 180);
        geometry_msgs::Pose pose;
        pose.position.x = msg->x;
        pose.position.y = msg->y;
        pose.position.z = 0;
        tf::Quaternion q;
        q.setRotation(tf::Vector3(0, 0, 1), world_pose[2]);
        pose.orientation.x = q.x();
        pose.orientation.y = q.y();
        pose.orientation.z = q.z();
        pose.orientation.w = q.w();
//        if (!world_pose_callback_.empty()) {
//            world_pose_callback_(pose);
//        }
    }

/***************************************************************************************************** 
 * Function:       // SendPoseAdjustRequest 
 * Description:    // 向机器人发送位置调整的请求，请求机器人以某个相对于当前位置的相对姿态调节自身位置  
 *                 // 例如:让机器人相对于当前位置前进1m并左旋30度,机器人调节完成后将发出“PoseAck”消息响应         
 * Publisher:      // pose_adjust_request_pub
 * Topic:          // "PoseRequest"
 * Input:          // int request_code:    1-基于里程计，借助底盘原始数据达到位置调节的功能，遥控和巡航模式通用
 *                 //                      2-基于SLAM，借助修正后的世界坐标调节机器人位置，一般用于巡航模式
 *                 // bool switch_x:       机器人x，即前后方向是否做调整 是-true 否-false
 *                 // bool switch_y:       机器人y，即左右方向是否做调整 是-true 否-false
 *                 // bool switch_th:      机器人z，即角度是否做调整 是-true 否-false
 *                 // double x_set:        x轴相对移动量(单位：m) +相对当前位置前进 -相对当前位置后退
 *                 // double y_set:        y轴相对移动量(单位：m) +相对当前位置左平移 -相对当前位置右平移
 *                 // double th_set:       z轴相对旋转量(单位：rad) +相对当前位置逆时针旋转 -相对当前位置顺时针旋转
 *                 // double max_vel_x:    x轴调整最大线速度(单位：m/s)  正值
 *                 // double max_vel_y:    y轴调整最大线速度(单位：m/s)  正值
 *                 // double max_vel_th:   z轴调整最大角速度(单位：rad/s) 正值
 *                 // double precision_x:  x轴调节精度(单位：m) 正值  基于里程计：一般最小0.003  基于世界坐标：一般最小0.008
 *                 // double precision_y:  y轴调节精度(单位：m) 正值  基于里程计：一般最小0.003  基于世界坐标：一般最小0.008
 *                 // double precision_th: z轴调节精度(单位：rad) 正值  基于里程计：一般最小0.1＊pi/180  基于世界坐标：一般最小0.25*pi/180
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 如果只调节一个或者两个轴，推荐将不调节的轴(switch)关闭
 *                 // 由于传感器精度的原因，机器人位置调整精度不宜过高，否则容易引起震荡
 *                 // 机器人完成位置调整后会有“PoseAck”响应，用户收到响应后即可进行下一步操作
*******************************************************************************************************/
    bool TurtlebotUser::sendPoseAdjustRequest(int request_code,
                                              bool switch_x, bool switch_y, bool switch_th,
                                              double x_set, double y_set, double th_set,
                                              double max_vel_x, double max_vel_y, double max_vel_th,
                                              double precision_x, double precision_y, double precision_th) {
      //tell the action client that we want to spin a thread by default
      std::cout<<"in sendPoseAdjustRequest"<<std::endl;
      //MoveBaseClient ac("move_base", true);
      ac = new MoveBaseClient("move_base", true);
       //wait for the action server to come up
      while(!ac->waitForServer(ros::Duration(5.0))){
        ROS_INFO("Waiting for the move_base action server to come up");
      }

      move_base_msgs::MoveBaseGoal goal;

      //we'll send a goal to the robot to move 1 meter forward
      goal.target_pose.header.frame_id = "base_link";
      goal.target_pose.header.stamp = ros::Time::now();

      goal.target_pose.pose.position.x = x_set;
      goal.target_pose.pose.position.y = y_set;
      goal.target_pose.pose.position.z = 0;

      tf::Quaternion q;
      q.setRotation(tf::Vector3(0, 0, 1), th_set);
      goal.target_pose.pose.orientation.x = q.x();
      goal.target_pose.pose.orientation.y = q.y();
      goal.target_pose.pose.orientation.z = q.z();
      goal.target_pose.pose.orientation.w = q.w();

      ROS_INFO("Sending goal");
     // ac.sendGoal(goal);
      ac->sendGoal(goal,
                      boost::bind(&TurtlebotUser::doneCb, this, _1, _2),
                      MoveBaseClient::SimpleActiveCallback(),
                      MoveBaseClient::SimpleFeedbackCallback());
   //   ac.waitForResult();

//      if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
//         ROS_INFO("Hooray, the base moved 1 meter forward");
//      else
//         ROS_INFO("The base failed to move forward 1 meter for some reason");

      return true;
    }
    void TurtlebotUser::doneCb(const actionlib::SimpleClientGoalState& state,
                               const move_base_msgs::MoveBaseResultConstPtr& result)
    {
      //std::cout<<"in doneCb"<<std::endl;
        PoseAckInfo ack;
        ROS_INFO("RESULT %s",state.toString().c_str());
       // boost::unique_lock<boost::mutex> lock(wp_mutex_);
        switch (state.state_) {
        case actionlib::SimpleClientGoalState::ABORTED:
        {
          ROS_INFO("NavigationManager::moveBaseResultCallback: ABORTED");
        }
          break;
        case actionlib::SimpleClientGoalState::SUCCEEDED:
        {

          if (!pose_adjust_ack_callback_.empty()) {
            ROS_INFO("NavigationManager::moveBaseResultCallback: SUCCEEDED");
              pose_adjust_ack_callback_(ack);
          }
        }
          break;
        default:
          break;
        }
     }

/***************************************************************************************************** 
 * Function:       // poseAckInfoCallBack 
 * Description:    // 机器人位置调整完成响应  
 *                 // 位置调整完成后，会回调该函数，用户进行下一步操作         
 * Subscriber:     // pose_adjust_ack_sub
 * Topic:          // "PoseAck"
 * Input:          // prosper_underpan::PoseAckInfo::ConstPtr& msg 完成消息响应
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 响应只针对最后一次位置调整请求有效
*******************************************************************************************************/
    void TurtlebotUser::poseAckInfoCallBack(const prosper_underpan::PoseAckInfo::ConstPtr &msg) {
        /*user code*/
      ROS_INFO("pose_adjust_ack_callback");
        PoseAckInfo ack;
        ack.ack_code = msg->ack_code;
        ack.success = msg->success;
        if (!pose_adjust_ack_callback_.empty()) {
            pose_adjust_ack_callback_(ack);
        }
    }

/***************************************************************************************************** 
 * Function:       // groupControlRequest 
 * Description:    // 群控系统自定义事件回调函数
 *                 // 如果用户收到的字符串与自定义的字符串user_evt_str相同，即可开始执行自己的程序         
 * Subscriber:     // group_control_sub
 * Topic:          // "GroupControlRequest"
 * Input:          // const prosper_underpan::GroupControlInfo::ConstPtr& msg 群控系统事件请求
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 如果接收到的字符串与自定义的字符串相同，用户开始执行自己的程序，如果程序中未定义此事件字符串，那么
 *                 // 程序将一直处于等待状态，需要群控系统人工给予机器人终止任务的信号
 *                 // 自定义事件完成后必须响应完成信号！
*******************************************************************************************************/
    void TurtlebotUser::groupControlRequestCallBack(const prosper_underpan::GroupControlInfo::ConstPtr &msg) {
        if (msg->request == 1 && msg->type == 0) {

          YAML::Node node = YAML::Load(msg->evt_str);

          if(node["task"].as<std::string>() == "set_getBucketArea"||node["task"].as<std::string>() == "set_putBucketArea"){ 
            if(!work_area_callbakc_.empty()) {
               
            WorkArea work_area;
            tf::Vector3 p1;
            p1.setX(node["p1"][0].as<double>());
            p1.setY(node["p1"][1].as<double>());
            p1.setZ(0.0);
            tf::Vector3 p2;
            p2.setX(node["p2"][0].as<double>());
            p2.setY(node["p2"][1].as<double>());
            p2.setZ(0.0);
            tf::Vector3 p3;
            p3.setX(node["p3"][0].as<double>());
            p3.setY(node["p3"][1].as<double>());
            p3.setZ(0.0);
            tf::Vector3 p4;
            p4.setX(node["p4"][0].as<double>());
            p4.setY(node["p4"][1].as<double>());
            p4.setZ(0.0);
            tf::Vector3 p5;
            p5.setX(node["p5"][0].as<double>());
            p5.setY(node["p5"][1].as<double>());
            p5.setZ(0.0);
 
            work_area.seq = node["id"].as<int>();
            work_area.type = node["type"].as<int>();
            work_area.dock = p5;
            work_area.area.push_back(p1);
            work_area.area.push_back(p2);
            work_area.area.push_back(p3);
            work_area.area.push_back(p4);
            work_area_callbakc_(work_area);
          }    
            GroupControlInfo ack;
            ack.evt_str = msg->evt_str;
            sendGroupControlAck(ack);
         }

          else{
            if (!group_control_callback_.empty()) {
                GroupControlInfo ctl;
                ctl.evt_str = msg->evt_str;
                ctl.request = msg->request;
                ctl.data = msg->data;
                ctl.type = msg->type;
                group_control_callback_(ctl);
            }
          }
        }
    }

/***************************************************************************************************** 
 * Function:       // SendGroupControlAck 
 * Description:    // 群控系统自定义事件完成响应
 *                 // 用户自定义事件完成以后，需要向群控系统响应完成事件的字符串         
 * Publisher:      // group_control_pub
 * Topic:          // "GroupControlAck"
 * Input:          // const RobotDriverBase::GroupControlInfo& ack
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 用户响应时必须保证user_evt_str和接收到的字符串一样
*******************************************************************************************************/
    bool TurtlebotUser::sendGroupControlAck(const GroupControlInfo &ack) {
        prosper_underpan::GroupControlInfo group_ctl_info;
        group_ctl_info.request = 0;
        group_ctl_info.type == 0;
        group_ctl_info.evt_str = ack.evt_str;
        group_control_pub_.publish(group_ctl_info);
        return true;
    }

/***************************************************************************************************** 
 * Function:       // emergentInfoCallBack 
 * Description:    // 群控系统紧急消息回调函数
 *                 // 用户收到紧急消息后  添加自己的安全保护代码         
 * Subscriber:     // emergent_info_sub
 * Topic:          // "EmergentInfo"
 * Input:          // const prosper_underpan::EmergentInfo::ConstPtr& msg 群控系统事件请求
 *                 // msg->cmd  0-取消任务 终止
 *                 // msg->cmd  1-中止任务 暂停
 *                 // msg->cmd  2-恢复任务 继续
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 为了保证系统安全，用户请务必添加紧急情况下的处理源码
*******************************************************************************************************/


    void TurtlebotUser::emergentInfoCallBack(const prosper_underpan::EmergentInfo::ConstPtr &msg) {
        EmergentInfo info;
        if(!emergent_callback_.empty()){
            info.cmd = msg->cmd;
            emergent_callback_(info);
        }
    }
/*****************************************************************************************************
* Function:       // TrailerModeCtl
* Description:    // 拖车模式切换
*                 // 机器人与拖车对接成功后 需向/tmp文件夹中写入拖车的长宽参数
* Input:          // trailer  1-进入拖车模式   0-退出拖车模式
*                 // trailer_width   拖车宽度 (m)
*                 // trailer_length  拖车长度 (m)
* Output:         // none
* Return:         // none
* Notice:         // 为了保证系统正常   拖车与机器人失去连接后记得调用该函数退出拖车模式
*******************************************************************************************************/
void  TurtlebotUser::TrailerModeCtl(int trailer,double trailer_width,double trailer_length){
    std::ofstream yaml_file("/tmp/trailer_info.yaml", std::ios::out | std::ios::binary);
    {
        YAML::Emitter out(yaml_file);
        out << YAML::BeginMap;
        out << YAML::Key << "trailer" << YAML::Value << trailer;
        out << YAML::Key << "trailer_width" << YAML::Value << trailer_width;
        out << YAML::Key << "trailer_length" << YAML::Value << trailer_length;
        out << YAML::EndMap;
    }

    yaml_file.close();
}

/*****************************************************************************************************
* Function:       // armAjustAckCallback
* Description:    // 机械臂调整完成后的相应
*                 // 调整完成后，会回调该函数，用户进行下一步操作
* Input:          // ArmAjustAck& ack 完成调整响应
* Output:         // none
* Return:         // none
*******************************************************************************************************/
    void TurtlebotUser::armAjustAckCallback(const ArmAjustAck& ack){
      if(!arm_adjust_ack_callback_.empty()){
        arm_adjust_ack_callback_(ack);
      }
    }

   bool TurtlebotUser::armExpend(){
     for(int i = 0;i<10;i++){
       write_data_[0] = 0xAA;
       write_data_[1] = 0x11;
       write_data_[2] = 0xBB;
       write_data_[3] = 0xCC;
       boost::system::error_code ec;
       int nn = sp_->write_some(boost::asio::buffer(write_data_, 4),ec);
       recv_data_ = false;
       std::cout<<"nnn:"<<nn<<std::endl;
       ros::Time begin = ros::Time::now();
       ros::Duration duration(0.1);
       while(1){

         if(ros::Time::now()-begin<duration){
            if(recv_data_ == true){
              return true;
            }
         }
         else{
           std::cout<<222<<std::endl;
           break;
         }


       }
     }
     if(recv_data_ == false){
       return false;
     }
     return false;
   }


    void TurtlebotUser::arm_working_exec(){
        ros::Rate rate(10);
        while(ros::ok()){
          if(listener_.waitForTransform("/map","/base_link",ros::Time(0),ros::Duration(0.2))){
            listener_.lookupTransform("/map","/base_link",ros::Time(0),world_pose);
            geometry_msgs::Pose pose;
            pose.position.x = world_pose.getOrigin().getX();
            pose.position.y = world_pose.getOrigin().getY();
            pose.position.z = world_pose.getOrigin().getZ();
            pose.orientation.x = world_pose.getRotation().getX();
            pose.orientation.y = world_pose.getRotation().getY();
            pose.orientation.z = world_pose.getRotation().getZ();
            pose.orientation.w = world_pose.getRotation().getW();
            if (!world_pose_callback_.empty()) {
              //std::cout<<"global_pose.x:::::"<<pose.position.x<<std::endl;
              //std::cout<<"global_pose.y:::::"<<pose.position.y<<std::endl;
                world_pose_callback_(pose);
            }
          }

            rate.sleep();
        }
    }
}




























